#include <string>
#include <ros/ros.h>
#include <sensor_msgs/JointState.h>
#include <tf/transform_broadcaster.h>
#include<stdio.h>
#include<map>
#include <sender/motor_data.h>
using namespace std;

std::string joints[]={
        "round_left_hip","left_hip_to_kneel","axis_to_left_hip","left_hip_to_shank","left_shank_to_foot","left_shank_pitch_foot",
        "round_right_hip","right_hip_to_kneel","axis_to_right_hip","right_hip_to_shank","right_shank_to_foot","right_shank_pitch_foot"
};
string mapping[20]={
        "0","left_hip_to_kneel","axis_to_left_hip","round_left_hip","left_hip_to_shank",
        "left_shank_to_foot","left_shank_pitch_foot","0","right_hip_to_kneel","axis_to_right_hip",
        "round_right_hip","right_hip_to_shank","right_shank_to_foot","right_shank_pitch_foot","0",
        "0","0","0","0","0"
};
class joint_map{
public:
    static    double k; //to radius
    string joint_name;
    int motor;
    double angle;
    double get_angle(){
        angle= (motor-512)*k;
        return angle;
    }
};
double joint_map::k=300/1024*3.14/180;//scale to radius
//tf var


ros::Publisher* joint_pub;

const double degree = M_PI/180;


int store_data[20]={0};
char out_msg[100];


void controlCallback(const sender::motor_data &data)
    {
	// robot state
    tf::TransformBroadcaster broadcaster;
    // message declarations
    geometry_msgs::TransformStamped odom_trans;
    sensor_msgs::JointState joint_state;
    odom_trans.header.frame_id = "axis";
    odom_trans.child_frame_id = "base_link";
        int ptr=0;
        for(int i=0;i<20;i++)
        {
            if(data.map[i])
                store_data[i]=data.data[i];
            ptr+=sprintf(out_msg+ptr,"%3d,",store_data[i]);
        }
        ROS_DEBUG( "%s",out_msg);

        //update joint_state

        joint_state.header.stamp = ros::Time::now();
        int n=sizeof(joints)/sizeof(joints[0]);
        joint_state.name.resize(n);
        joint_state.position.resize(n);
        double k=300.0/1024.0*3.1415926/180;

        int t=store_data[4];
        store_data[4]=-store_data[1]+store_data[4];
        store_data[5]+=t;


        t=store_data[11];
        store_data[11]=-store_data[8]+store_data[11];
        store_data[12]+=t;
        // ROS_INFO("11:%d",t);
        for(int i=0,j=0;i<20;i++){
            if(mapping[i]=="0")
                continue;

            joint_state.name[j]=mapping[i];
            joint_state.position[j]=(double)(store_data[i])*300/1024*3.1415926/180;
            j++;
            //   ROS_INFO("%s:%d->%.3f,",mapping[i].data(), store_data[i],joint_state.position[j]);
        }

        // update transform
        // (moving in a circle with radius=2)

        odom_trans.header.stamp = ros::Time::now();
        /*
                odom_trans.transform.translation.x = 1;
                odom_trans.transform.translation.y = 1;
                odom_trans.transform.translation.z = 1;*/
        odom_trans.transform.rotation = tf::createQuaternionMsgFromYaw(0);

        //send the joint state and transform
        joint_pub->publish(joint_state);
        broadcaster.sendTransform(odom_trans);
    };
int main(int argc, char** argv) {
    ros::init(argc, argv, "motor_driver");
	ROS_INFO("init done!");
    ros::NodeHandle n;
    ros::Publisher pub= (n.advertise<sensor_msgs::JointState>("joint_states", 10));
    joint_pub=&pub;
    //init var

    ros::Subscriber motor_sub=n.subscribe("motion/motor",10,controlCallback);
    ros::spin();

    return 0;

}
